#include "robot_state.h"

/**
 * @brief 重置机器人状态到初始值（实现声明的 reset 函数）
 */
void RobotState::reset() {
    is_fault = false;          // 重置为无故障
    is_moving = false;         // 重置为静止
    end_position = QVector3D(0.0, 0.0, 0.0);  // 位置重置到原点
    end_velocity = QVector3D(0.0, 0.0, 0.0);  // 速度重置为0
    timestamp = 0;             // 时间戳重置为0
    error_code = 0;            // 错误码重置为0（无错误）
}

/**
 * @brief 拷贝当前状态（实现声明的 copy 函数）
 * @return RobotState - 新的状态副本
 */
RobotState RobotState::copy() const {
    RobotState copied_state;   // 创建新的状态对象

    // 逐个拷贝成员变量（确保深拷贝，QVector3D 自带深拷贝）
    copied_state.is_fault = this->is_fault;
    copied_state.is_moving = this->is_moving;
    copied_state.end_position = this->end_position;
    copied_state.end_velocity = this->end_velocity;
    copied_state.timestamp = this->timestamp;
    copied_state.error_code = this->error_code;

    return copied_state;
}

/**
 * @brief 判断是否故障（实现声明的 isFault 函数）
 * @return bool - 故障状态
 */
bool RobotState::isFault() const {
    // 可扩展逻辑：如结合 error_code 判断具体故障类型
    return this->is_fault || (this->error_code != 0);
}

/**
 * @brief 判断是否运动中（实现声明的 isMoving 函数）
 * @return bool - 运动状态
 */
bool RobotState::isMoving() const {
    // 可扩展逻辑：如判断速度是否大于阈值（避免微小抖动误判）
    const double velocity_threshold = 0.001;  // 速度阈值（单位：m/s，可调整）
    return this->is_moving && (this->end_velocity.length() > velocity_threshold);
}